#include "NXT++.h"
#include "iostream"
#include "cstdlib"
#include "string"
#include "windows.h"
#include "fstream"

using namespace std;

Comm::NXTComm comm; //NXT Connection

//Value of the array determint the type or position of the Motor/Sensor

int Motor[3]; /*Motor array 0-LeftMotor
			                1-RightMotor
							2-OtherMotor*/
/*Array Position
Array:| OUT_A | OUT_B | OUT_C |*/
int Sensor[4]; /*Sensor array 0-Ultrasonic
			                  1-LightSensor
							  2-SoundSensor
							  3-TouchSensor*/
/*Array Position
Array: | IN_1 | IN_2 | IN_3 | IN_4 |*/
//Help Function
void help();
//Command Execution Function
void exec(string line);
//Get Command Function
void get_cmd();
//Close all
void closeall();
//Connection Type
int ConnType;
//Loop Exit Flag
bool prog_exit=false;
//Choice for Help
char choice;
//Command Line
//Speed Integer
int speed;
//Motor Time
int time_m;
//Turn time
int turn_t;
//Turn Degree
int degree_t;
int left_m,right_m,other_m;
//Vars for Sensor address
int sonar(0),light(0),sound(0),touch(0);
//Start Motor/Sensor Reconize
string line;
bool conn;
//NXT Variables
int battery;
double version;
double version_p;
double memory;

void BTConnection()
{
	conn = NXT::OpenBT(&comm);
	if(conn) //initialize the NXT and continue if it succeeds
    {
		cout << "Connection Established" << endl;
		cout << "Do you want instructions?" << endl;
		cin >> choice;
		if(choice == 'y' || choice == 'Y')
		{
			help();
		}
		cout << "Program will now record the Motor and Sensor Position." << endl;
		cout << "-------------------------------------------------------------------" << endl;
		cout << "Motor: 0-LeftMotor | 1-RightMotor | 2-OtherMotor" << endl;
		cout << "OUT_A: "; cin >> Motor[0]; cout << endl;
		cout << "OUT_B: "; cin >> Motor[1]; cout << endl;
		cout << "OUT_C: "; cin >> Motor[2]; cout << endl;
		cout << "-------------------------------------------------------------------" << endl;
		cout << "Sensor: 0-Ultrasonic | 1-LightSensor | 2-SoundSensor | 3-TouchSensor" << endl;
		cout << "IN_1: "; cin >> Sensor[0]; cout << endl;
		cout << "IN_2: "; cin >> Sensor[1]; cout << endl;
		cout << "IN_3: "; cin >> Sensor[2]; cout << endl;
		cout << "IN_4: "; cin >> Sensor[3]; cout << endl;
		//-----------------------------END SENSOR/MOTOR RECORD-----------------------------
		/*Functions: 1-Move Robot Stright Forward 2-Move Robot Stright Backward 3-Turn Left 4-Turn Right
		  5-Left Turn Backward 6-Right Turn Backward 7-Move Single Motor*/
		//Vars for right left and other address
		for(int x = 0; x < 3; x++)
		{
			if(Motor[x] == 0)//If 0 it is the left
			{
				left_m = x;
			}
			else if(Motor[x] == 1)
			{
				right_m = x;//If 1 it is right
			}
			else if(Motor[x] == 2)
			{
				other_m = x;//Other motor
			}
		}
		for(int z = 0; z < 4; z++)
		{
			if(Sensor[z] == 0)
			{
				sonar = z;
			}
			else if(Sensor[z] == 1)
			{
				light = z;
			}
			else if(Sensor[z] == 2)
			{
				sound = z;
			}
			else if(Sensor[z] == 3)
			{
				touch = z;
			}
		}
		//End Motor/Sensor Reconize
		//Starting Sonsole
		battery = NXT::BatteryLevel(&comm);
		version = NXT::GetFirmwareVersion(&comm);
		version_p = NXT::GetProtocolVersion(&comm);
		memory = NXT::GetAvailableFlash(&comm);
		system("pause");
		system("cls");
		cout << "|-------------NXT Robot Console By Siyuan Gao Copyright(c)NCHUnion-------------|" << endl;
		cout << "Battery Level:" << battery << "|Version:" << version << "|Prptocol Version:" << version_p << "|Available Flash:" << memory << "KB" << endl << endl;
		get_cmd();
	}
	else
	{
		cout << "NXT Connection Failed!" << endl;
		system("pause");
	}
}
void USBConnection()
{
	conn = NXT::Open(&comm);
	if(conn) //initialize the NXT and continue if it succeeds
    {
		cout << "Connection Established" << endl;
		cout << "Program will now record the Motor and Sensor Position." << endl;
		cout << "-------------------------------------------------" << endl;
		cout << "Motor: 0-LeftMotor | 1-RightMotor | 2-OtherMotor" << endl;
		cout << "OUT_A: "; cin >> Motor[0]; cout << endl;
		cout << "OUT_B: "; cin >> Motor[1]; cout << endl;
		cout << "OUT_C: "; cin >> Motor[2]; cout << endl;
		cout << "--------------------------------------------------------------------" << endl;
		cout << "Sensor: 0-Ultrasonic | 1-LightSensor | 2-SoundSensor | 3-TouchSensor" << endl;
		cout << "IN_1: "; cin >> Sensor[0]; cout << endl;
		cout << "IN_2: "; cin >> Sensor[1]; cout << endl;
		cout << "IN_3: "; cin >> Sensor[2]; cout << endl;
		cout << "IN_4: "; cin >> Sensor[3]; cout << endl;
		//-----------------------------END SENSOR/MOTOR RECORD-----------------------------
	}
	else
	{
		cout << "NXT Connection Failed!" << endl;
		system("pause");

	}
	NXT::Close(&comm);
}
int main()
{
	system("color 0a");
	cout << "                         ";
	cout << "Welcome to Siyuan's NXT Console" << endl;
	cout << "================================================================================" << endl;
	cout << "                       ";
	cout << "Please select your connection type: " << endl;
	cout << "(1)Bluetooth Connection" << endl;
	cout << "(2)USB Connection" << endl;
	cout << "(3)Exit" << endl;
	cin >> ConnType;
	switch(ConnType)
	{
	case 1:
		cout << "Connecting to Lego NXT, this might take some time." << endl;
		BTConnection();
		break;
	case 2:
		USBConnection();
		break;
	case 3:
		return 0;
		break;
	default:
		cout << "Invaild Connection Type!" << endl;
	}
    return 0;
}

void help()
{
	cout << "                         ";
	cout << "Welcome to NXT Console by Siyuan" << endl;
	cout << "Command List:" << endl;
	cout << "-------------"<< endl;
	cout << "Motor:" << endl;
	cout << "Forward();" << endl;
	cout << "Backward();" << endl;
	cout << "TurnLeft();" << endl;
	cout << "TurnRight();" << endl << endl;
	cout << "OtherMotor();" << endl;
	cout << "Sensor Command:" << endl;
	cout << "---------------" << endl;
	cout << "GetLight();" << endl;
	cout << "GetTouch();" << endl;
	cout << "GetSound();" << endl;
	cout << "GetSonar();" << endl;
	cout << "SaveLight();" << endl;
	cout << "SaveTouch():" << endl;
	cout << "SaveSound();" << endl;
	cout << "SaveSonar();" << endl;
	cout << "Special Command:" << endl;
	cout << "----------------" << endl;
	cout << "Kill();" << endl;
	cout << "RunProg();" << endl;
	cout << "SelfDestruct();" << endl;
	cout << "Cls();" << endl;
	cout << "Exit();" << endl;
	cout << "ConnClose();" << endl;
	cout << "ReConn();" << endl;
	cout << "Cls();" << endl;
	cout << "Help();" << endl;
	cout << "GetStatus();" << endl;
	cout << endl;
}
void get_cmd()
{
	do
		{
			getline(cin,line);
			exec(line);
		}
		while(!prog_exit);
}
void exec(string line)
{
	if(line == "")
	{
		cout << ">>>";
	}
	else if(line == "Forward();")
	{
		cout << "Please input the speed you want robot to drive(Power Persent 1-100): ";
		cin >> speed; cout << endl;
		if(speed <=100)
		{
		cout << "How long you want the robot to drive (In millisecond): ";
		cin >> time_m; cout << endl;
		NXT::Motor::SetForward(&comm,left_m,speed);
		NXT::Motor::SetForward(&comm,right_m,speed);
		Wait(time_m);
		NXT::Motor::Stop(&comm,left_m,false);
		NXT::Motor::Stop(&comm,right_m,false);
		}
		else
		{
			NXT::PlayTone(&comm,523,100);
			Wait(40);
			NXT::PlayTone(&comm,587,100);
			Wait(40);
			NXT::PlayTone(&comm,659,100);
			//String
			string port_r,port_l;
			if(left_m == 0)
			{
				port_l = "OUT_A";
			}
			else if(left_m == 1)
			{
				port_l = "OUT_B";
			}
			else if (left_m == 2)
			{
				port_l = "OUT_C";
			}
			else
			{
				port_l = "UNKNOWN";
			}
			//Left Motor
			if(right_m == 0)
			{
				port_r = "OUT_A";
			}
			else if(right_m == 1)
			{
				port_r = "OUT_B";
			}
			else if (right_m == 2)
			{
				port_r = "OUT_C";
			}
			else
			{
				port_r = "UNKNOWN";
			}
			cout << "Runtime Error!" << endl;
			cout << "Forward(" << port_l << "," << port_r << ")::Speed: " << speed << endl;
			cout << "-----------------------------^" << endl;
			cout << "Power needs between 1-100" << endl;
		}
	}
	else if(line == "Backward();")
	{
		cout << "Please input the speed you want robot to drive(Power Persent 1-100): ";
		cin >> speed; cout << endl;
		if(speed <=100)
		{
		cout << "How long you want the robot to drive (In millisecond): ";
		cin >> time_m; cout << endl;
		NXT::Motor::SetReverse(&comm,left_m,speed);
		NXT::Motor::SetReverse(&comm,right_m,speed);
		Wait(time_m);
		NXT::Motor::Stop(&comm,left_m,false);
		NXT::Motor::Stop(&comm,right_m,false);
		}
		else
		{
			NXT::PlayTone(&comm,523,100);
			Wait(40);
			NXT::PlayTone(&comm,587,100);
			Wait(40);
			NXT::PlayTone(&comm,659,100);
			string port_r,port_l;
			if(left_m == 0)
			{
				port_l = "OUT_A";
			}
			else if(left_m == 1)
			{
				port_l = "OUT_B";
			}
			else if (left_m == 2)
			{
				port_l = "OUT_C";
			}
			else
			{
				port_l = "UNKNOWN";
			}
			//Left Motor
			if(right_m == 0)
			{
				port_r = "OUT_A";
			}
			else if(right_m == 1)
			{
				port_r = "OUT_B";
			}
			else if (right_m == 2)
			{
				port_r = "OUT_C";
			}
			else
			{
				port_r = "UNKNOWN";
			}
			cout << "Runtime Error!" << endl;
			cout << "Backward(" << port_l << "," << port_r << ")::Speed: " << speed << endl;
			cout << "-----------------------------^" << endl;
			cout << "Power needs between 1-100" << endl;
		}
	}
	else if(line == "TurnLeft();")
	{
		double temp;
		cout << "Please input the degree you want robot to turn: ";
		cin >> degree_t; cout << endl;
		temp = 8.333 * degree_t + 300;
		turn_t = int(temp);
		NXT::Motor::SetForward(&comm,right_m,50);
		Wait(turn_t);
		NXT::Motor::Stop(&comm,right_m,false);
	}
	else if(line == "TurnRight();")
	{
		double temp;
		cout << "Please input the degree you want robot to turn: ";
		cin >> degree_t; cout << endl;
		temp = 8.333 * degree_t + 300;
		turn_t = int(temp);
		NXT::Motor::SetForward(&comm,left_m,50);
		Wait(turn_t);
		NXT::Motor::Stop(&comm,left_m,false);
	}
	else if(line == "GetStatus();")
	{
		int port_a,port_b,port_c;
		port_a = NXT::Motor::GetRotationCount(&comm,0);
		port_b = NXT::Motor::GetRotationCount(&comm,1);
		port_c = NXT::Motor::GetRotationCount(&comm,2);
		battery = NXT::BatteryLevel(&comm);
		version = NXT::GetFirmwareVersion(&comm);
		version_p = NXT::GetProtocolVersion(&comm);
		memory = NXT::GetAvailableFlash(&comm);
		cout << "NXT Robot Status:" << endl; cout << "-----------------" << endl;
		cout << "OUT_A Rotation: " << port_a << endl;
		cout << "OUT_B Rotation: " << port_b << endl;
		cout << "OUT_C Rotation: " << port_c << endl;
		cout << "Battery Level: " << battery << " millivolts." << endl;
		cout << "Firmware Version: " << version << endl;
		cout << "Protocol Version: " << version_p << endl;
		cout << "Available Flash: " << memory << "KB" << endl;
		cout << ">>>";
	}
	else if(line == "OtherMotor();")
	{
		cout << "Please input the speed you want robot to drive(Power Persent 1-100): ";
		cin >> speed; cout << endl;
		if(speed <=100)
		{
		cout << "How long you want the robot to drive (In millisecond): ";
		cin >> time_m; cout << endl;
		NXT::Motor::SetForward(&comm,other_m,50);
		Wait(time_m);
		NXT::Motor::Stop(&comm,other_m,false);
		}
		else
		{
			NXT::PlayTone(&comm,523,100);
			Wait(40);
			NXT::PlayTone(&comm,587,100);
			Wait(40);
			NXT::PlayTone(&comm,659,100);
			string port_o;
			if(other_m == 0)
			{
				port_o = "OUT_A";
			}
			else if(other_m == 1)
			{
				port_o = "OUT_B";
			}
			else if (other_m == 2)
			{
				port_o = "OUT_C";
			}
			else
			{
				port_o = "UNKNOWN";
			}
			cout << "Runtime Error!" << endl;
			cout << "OtherMotor(" << port_o << ")::Degree: " << degree_t << endl;
			cout << "------------------------^" << endl;
			cout << "Degree needs between 1-360" << endl;
		}
	}
	else if(line == "GetLight();")
	{
		int light_s;
		int light_data;
		int light_poll;
		cout << "You are accessing light sensor, please select mode:" << endl;
		cout << "(1)Reflected Light   (Sensor will turn on the light)" << endl;
		cout << "(2)Ambient Light     (Sensor will not turn on the light)" << endl;
		cin >> light_s;
		switch(light_s)
		{
		case 1:
			NXT::Sensor::SetLight(&comm,light,true);
			break;
		case 2:
			NXT::Sensor::SetLight(&comm,light,false);
			break;
		default:
			cout << "Incorrect Parameter! By program default NXT Console will turn on Reflected Mode." << endl;
			NXT::Sensor::SetLight(&comm,light,true);
		}
		cout << "Please input how long you want the program to poll data (Seconds): ";
		cin >> light_poll; cout << endl;
		cout << "Polling data........." << endl << endl;
		for(int x = 1; x<=light_poll; x++)
		{
			light_data = NXT::Sensor::GetValue(&comm,light);
			cout << "Second: " << x << " Data: " << light_data << endl;
			Wait(1000);
		}
		cout << endl << "Data Polling Complete!" << endl;
		NXT::Sensor::SetLight(&comm,light,false);
	}
	else if(line == "GetTouch();")
	{
		int touch_poll;
		bool touch_data;
		NXT::Sensor::SetTouch(&comm,touch);
		cout << "Please input how long you want the program to poll data (Seconds): ";
		cin >> touch_poll; cout << endl;
		cout << "Polling data........." << endl << endl;
		for(int x = 1; x<=touch_poll; x++)
		{
			touch_data = NXT::Sensor::GetValue(&comm,touch);
			cout << "Second: " << x << " Data: " << touch_data << endl;
			Wait(1000);
		}
		cout << endl << "Data Polling Complete!" << endl << endl;
	}
	else if(line == "GetSound();")
	{
		int sound_poll;
		int sound_data;
		NXT::Sensor::SetSound(&comm,sound);
		cout << "Please input how long you want the program to poll data (Seconds): ";
		cin >> sound_poll; cout << endl;
		cout << "Polling data........." << endl << endl;
		for(int x = 1; x<=sound_poll; x++)
		{
			sound_data = NXT::Sensor::GetValue(&comm,sound);
			cout << "Second: " << x << " Data: " << sound_data << endl;
			Wait(1000);
		}
		cout << endl << "Data Polling Complete!" << endl << endl;
	}
	else if(line == "GetSonar();")
	{
		int sonar_poll;
		int sonar_data;
		NXT::Sensor::SetSonar(&comm,sonar);
		cout << "Please input how long you want the program to poll data (Seconds): ";
		cin >> sonar_poll; cout << endl;
		for(int x = 1; x<=sonar_poll; x++)
		{
			sonar_data = NXT::Sensor::GetSonarValue(&comm,sonar);
			cout << "Second: " << x << " Data: " << sonar_data << " cm" << endl;
			Wait(1000);
		}
		cout << endl << "Data Polling Complete!" << endl << endl;
	}
	else if(line == "SaveLight();")
	{
		int light_s;
		int light_data;
		int light_poll;
		ofstream light_f;
		light_f.open("light.log",ios::app);
		cout << "You are accessing light sensor, please select mode:" << endl;
		cout << "(1)Reflected Light   (Sensor will turn on the light)" << endl;
		cout << "(2)Ambient Light     (Sensor will not turn on the light)" << endl;
		cin >> light_s;
		switch(light_s)
		{
		case 1:
			NXT::Sensor::SetLight(&comm,light,true);
			break;
		case 2:
			NXT::Sensor::SetLight(&comm,light,false);
			break;
		default:
			cout << "Incorrect Parameter! By program default NXT Console will turn on Reflected Mode." << endl;
			NXT::Sensor::SetLight(&comm,light,true);
		}
		cout << "Please input how long you want the program to poll data (Seconds): ";
		cin >> light_poll; cout << endl;
		cout << "Polling data........." << endl << endl;
		for(int x = 1; x<=light_poll; x++)
		{
			light_data = NXT::Sensor::GetValue(&comm,light);
			light_f << endl << "New Data:" << endl;
			light_f << "Second: " << x << " Data: " << light_data << endl;
			Wait(1000);
		}
		cout << endl << "Data Polling Complete!" << endl;
		NXT::Sensor::SetLight(&comm,light,false);
	}
	else if(line == "Help();")
	{
		help();
	}
	else if(line == "Cls();")
	{
		system("cls");
		cout << "|-------------NXT Robot Console By Siyuan Gao Copyright(c)NCHUnion-------------|" << endl;
		battery = NXT::BatteryLevel(&comm);
		version = NXT::GetFirmwareVersion(&comm);
		version_p = NXT::GetProtocolVersion(&comm);
		memory = NXT::GetAvailableFlash(&comm);
		cout << "Battery Level:" << battery << "|Version:" << version << "|Prptocol Version:" << version_p << "|Available Flash:" << memory << "KB" << endl << endl;
		cout << ">>>";
	}
	else if(line == "ConnClose();")
	{
		cout << endl << "NXT Console is closing NXT Connection. Please wait..." << endl;
		NXT::Close(&comm);
		cout << endl << "NXT Console has closed connection with NXT. To researt use ReConn();" << endl << endl;
		cout << ">>>";
	}
	else if(line == "Exit();")
	{
		closeall();
	}
	else if(line == "ReConn();")
	{
		cout << "                         ";
		cout << "Welcome to Siyuan's NXT Console" << endl;
		cout << "================================================================================" << endl;
		cout << "                       ";
		cout << "Please select your connection type: " << endl;
		cout << "(1)Bluetooth Connection" << endl;
		cout << "(2)USB Connection" << endl;
		cout << "(3)Exit" << endl;
		cin >> ConnType;
		switch(ConnType)
		{
				case 1:
				cout << "Connecting to Lego NXT, this might take some time." << endl;
				BTConnection();
			break;
				case 2:
				USBConnection();
			break;
				case 3:
				exit(0);
				break;
			default:
				cout << "Invaild Connection Type!" << endl;
				exit(0);
		}
	}
	else
	{
		cout << "Unknown Command Please check your command." << endl;
		cout << "Unknown Command: " << line << endl;
		cout << "-----------------^" << endl;
		cout << ">>>";
	}
	get_cmd();
}
void closeall()
{
	cout << endl << "NXT Console is closing......Unload everything." << endl;
	Sleep(2000);
	exit(0);
}